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1  Introduction 

This  report  summarizes  Robil’s  activities  during  the  period  of  January  -  April  2013. 

Robil’s  team  efforts  focused  in  the  following  areas: 

•  Infrastructure  Organization:  including  definition  of  the  software  interfaces,  software 
testing  and  debugging. 

•  Operational: Functional  Analysis  of  the  three  events 

•  Perception:  Five  modules  have  been  defined  and  partially  implemented 

•  Decision  Making:  An  execution  engine  that  allows  partial  plans  (behavior  trees)  to  be 
composed  into  full  plans  that  can  be  executed  at  runtime  has  been  implemented 

•  Dismounted  Mobility:  four  walking  modes  have  been  developed  to  facilitate  crossing 
different  types  of  terrain.  Two  modes,  the  continuous  dynamic  walk  and  the  pentad 
walk  allowed  successful  completion  of  task-2  as  demonstrated  in  the  video  available 
at:  http://www.youtube.com/watch?v=TQwOfpzHg7c  and  further  detailed  in  section 
3.4.1.  A  dynamic  method  for  entering  the  car  has  been  developed  as  demonstrated  in 
the  video  available  at:  http://www.youtube.com/watch?v=8QD9-YUzhngand 
further  detailed  in  section  3.4.2.  The  modular  software  architecture  that  supports  the 
different  walking  modes  is  detailed  in  section  3.4.3. 

•  Mounted  Mobility:  Driving  capabilities  have  been  implemented  and  tested.  Video  is 
available  at:  http://www.youtube.com/watch?v=820i  V4PF80 

•  Dexterity:  focus  on  single  arm  motion  and  grasping  strategies  and  telerobotic 
operation. 

A  follow-up  working  workshop  took  place  at  the  Technion  in  March  20,  2013. 

Robil  team  has  been  working  in  fulfilling  the  task  requirements  for  the  qualifiers. 

In  parallel  the  integration  has  continued  and  is  tested  into  the  CloudSim  environment. 

1  Methods,  Assumptions,  and  Procedures 
1.1  Assumptions 

Our  assumptions  are  based  upon  the  details  published  by  DARPA's  official  releases  of  the 
VRC  Rules  and  VRC  Technical  Guide  document.  We  had  also  used  the  DRC  forum  and  FAQ 
to  dig  for  information  as  many  of  the  details  were  unknown  or  not  final  during  the  whole 
period  of  design  and  development  of  the  software. 
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One  major  assumption  which  is  necessary  to  emphasize  is  that  no  team  may  compete  with 
simulator  parameters  different  than  those  of  the  official  Gazebo.  That  is,  no  one  can  optimize 
solver  step-size  or  number  of  iterations  to  fit  his  needs. 

1.2  Methods 

1.2.1  System  Capabilities 

Utilizing  ROS  architecture,  we  have  denoted  a  set  of  well-defined  capabilities  which  in 
different  compositions  forms  complex  behaviors.  Each  capability  shall  be  implemented  as  a 
ROS  package  by  its  designated  group,  and  react  as  autonomous  unit  of  the  complete  system, 
and  with  the  ability  to  request  services  from  other  packages.  A  definition  of  all  the 
capabilities  relevant  for  the  VRC  can  be  found  in  Table  1. 

TABLE  1:  ROBIL  SYSTEM  CAPABILITIES  DEFINED  FOR  VRC 


Task 

Symbol 

Capability  Name 

Description 

T1  Operation 

Cll 

Operator  control 

HMI  unit  for  event  control  by  intervention  in  mission 
planning  and  execution  (T3)  processes  and  level  of 
autonomy. 

Agent 

Used  as  a  gateway  between  the  OCU  and  the  robot 
at  the  robot's  side.  Deliver  all  messages  to  and  from 

the  OCU  and  controls  the  communications. 

T2 

Perception 

C21 

Vision  and  Lidar 

Operate  camera  and  Lidar  to  capture  scene  at  a 
requested  azimuth. 

C22 

Ground  Recognition  and 
Mapping 

get  ground  position  and  surface  shape 

C23 

Object  recognition 

Search  for  a  specific  object  in  an  image 

C24 

Obstacle  Detection 

Detect  obstacles  on  ground  and  above  ground 

C25 

Global  Position 

Find  robot  relative  position  in  global  map  and 
process  IMU  data 

T3  Decision- 

Making 

C31 

Path  Planning 

Search  for  a  global  route  on  a  map 

C34 

Mission  planner  and 

executor 

Behavior  Trees  planning  and  execution 

C35 

Mission  Monitoring 

Monitoring  task  progress  by  condition  validation 

T4 

Dismounted 

Mobility 

C41 

Quasi  Static  control 

1.  Foot  placement;  2.  Single  step;  3.  Turn  in  place 

C42 

Dynamic  Locomotion 

Walk  by  requested  path\direction  and  maintain 
stability  all  times.  4  modes: 

1.  Continuous  Dynamic;  2.  Discrete  Foot  Placement 
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(including  FP  planning);  3.  Panted  walk; 

4.  Translation  \  Rotation  behavior 

C45 

Posture  Control 

Control  head  and  torso  movements 

C46 

Mount  Vehicle 

Get  the  robot  from  standing  outside  the  vehicle  to  a 
safe  sitting  position  inside  it 

C47 

Dismount  Vehicle 

Get  the  robot  from  sitting  in  the  vehicle  to  a 
standing  position  outside  it. 

C48 

Stand  Up 

Stand  up  robot  in  case  of  falling 

T5  Mounted 

Mobility 

C51 

Car  operation 

Drive  and  navigate  the  car  based  on  vision. 

T6  Dexterity 

C65 

Connect  hose  to  spigot 
(high  level  dexterity) 

Plan  and  follow  procedure  to  connect  hose  to  spigot 

C66 

Grasp  and  release  an 
Object  (low  level 

dexterity) 

Operate  hand  to  hold  an  object  and  release  by 

command. 

C67 

Car  Manipulation  (high 
level  dexterity) 

Perform  manipulations  by  request  from  C51  to  drive 
car  (car  ignition,  steering  wheel  and  gear) 
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1.3  Integration 

The  integration  was  responsible  of  making  sure  that  interfaces  are  properly  defined  and  then 
that  the  implementation  matches  needed  functionality. 

For  ROBIL  project,  several  teams  were  working  distributed  and  in  parallel.  Each  team  was 
responsible  of  one  or  more  ROS  packages.  The  integration  checks  that  ROS  packages  are 
interacting  as  defined  in  system  functional  requirements. 

We  used  the  following  tools:  git,  github,  ienkins 

The  workflow  for  integrating  a  package  is  as  follows: 

1.  The  official  code  repository  is  in  the  github  (known  as  official  tree). 

2.  Each  team  has  a  corresponding  repository  in  the  github  (known  as  team 
repository) 

3.  The  code  to  integrate  should  be  in  the  local  git  repository 

4.  The  code  has  to  be  pushed  to  the  team  repository  in  the  github  and  a  pull 
request  has  to  be  opened  to  integrator. 

5.  Jenkins  server  is  then  activated: 

6.  Pull  the  code 

7.  Compile  it. 

8.  Perform  sanity  tests 

9.  Integrator  then  pull  the  new  code  and  makes  its  own  checks:  functionality, 
bugs  correction,  integration 

10.  Issues  are  opened  and  tracked  via  trac  server 

11.  If  the  new  code  is  good  enough,  it  is  pushed  to  the  official  tree. 

Debugging  sessions  are  conducted  with  Google  hangout  since  teams  are  geographically 
spread  over  the  country. 

We  have  added  the  cloudsim  component  of  the  tests: 

1.  1.  Jenkins  server  executes  tests  there  on  the  field  computer  while  the  simulator 
computer  is  running  the  drcsim. 

2.  A  script  analyzes  the  logs  in  order  to  determine  if  tests  have  passedFigure  1 
presents  the  integration  and  development  Gantt  of  the  last  quarter  in  regard 
tothe  program  schedule. 
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2  Results  and  Discussion 
2.1  Operation 

2.1.1  Operational  Concept  Definition  (OCD)  summary 

•  ROBIL  looks  for  a  mission-specific  set  of  data  (for  example  car,  door,  steering  wheel 
to  find  driver  seat  of  a  car)  and  when  the  set  is  found,  it  is  sent  to  the  operator  as 
vector  data  over  visual  data  for  approval/correction. 

•  Minimal  User  intervention  is  achieved  by  ROBIL  transmitting  back  visual 
information  with  perceived  mission  plan  in  predefined  decision  points  or  when  no 
solution  is  found. 

•  Only  mission  data  (path,  object  recognition  etc.)  or  predefined  action  command 
sequences  are  transmitted  to  ROBIL  by  the  operator. 

•  Tethering  shall  only  be  used  when  no  solution  is  found  and  only  per  specific  segment 
(for  example  -  tethered  hand  operation  while  keeping  stability  and  mobility  modules 
autonomous). 

•  Behavior  tweaking  is  done  using  sliders  of  shifting  the  weight  of  different  variables  to 
encourage  Faster/Safer  operation,  more/less  feedback  and  more/less  autonomy. 

•  A  bank  of  preset  motion  sequences  and  sensor  operations  shall  be  available  to  the 
operator  at  all  times  (stop,  kneel,  sit,  get  image,  get  obstacle  map  etc.) 

•  Ground  rules  of  operation: 

o  No  operator  override  for  10  seconds  =  approval 
o  Operator  intervention  starts  =  ROBIL  stops  and  waits 
o  Operator  transmission  shall  be  answered  -  Received  and  Done 

2.1.2  Implemented  Functionality: 

•  Task  selection 

•  Execution  control  (run,  pause,  resume) 

•  Receiving  images 

•  Receiving  occupancy  grid 

•  Receiving  path 

•  Operator  requests 

•  Override  and  draw  new  path 

•  Robot  status,  location  and  orientation 

•  View  updated  Behavior  tree 
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2.1.3  HMI  architecture 

The  architecture  of  the  Cll  module  consists  of  two  units  -  Cll  Operator  Control,  the  OCU 
computer  at  the  operator  side,  and  Cll  Agent  which  is  at  the  robot  side  (Figure  2). 


Figure  3.1.1:  HMI  architecture 


2.1.4  OCU  Design 

Figure  3  demonstrates  the  GUI  at  the  OCU  unit.  The  GUI  is  divided  into  3  segments: 

•  Left:  Mission  control-  where  the  operator  can  tweak  the  robot's  behavior,  control 
autonomy  levels  and  request  for  information. 

•  Middle:  Image  bank 

•  Right:  Occupancy  grid  with  overlaying  vector  data  of  robot  position,  orientation  and 
the  planned  path.  The  operator  may  change  robot's  scene  understanding  by  adding  or 
editing  vector  data. 
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Figure  3.1.2:  OCU:  GUI  design 

11 

(Jobil 


Distribution  A:  Approved  for  public  release;  distribution  is  unlimited. 


DARPA  —  Tactical  Technology  Office  (TTO) 

AWARD  NO.  FA8655-12-1-2143 

DARPA-BAA- 12-39  -  “DARPA  Robotics  Challenge” 

Project  Name:  ROBIL 

2.1.5  Operation  at  the  VRC 

The  final  operation  concept,  which  was  used  at  the  VRC  runs,  was  based  upon  the 
understanding  that  much  of  our  planned  autonomous  capabilities  weren't  finalized.  After 
loading  the  mission,  the  operator  usually  requested  a  picture  and  obstacle  grid  from  the  robot. 
On  the  received  grid  the  operator  draw  a  path  to  get  out  from  the  starting  pen,  sent  it  to  the 
robot  and  waited  for  a  confirmation  when  done.  The  next  step  was  to  request  a  picture  and 
grid  again,  and  upon  the  current  task  to  continue  with  giving  paths  and  running  modules  to 
enter  vehicle,  crawl  into  mud  or  pick  up  the  hose.  In  case  that  ATLAS  fell  down,  a  stand-up 
script  was  launched. 

2.2  Perception 

The  workload  was  distributed  between  five  nodes  (Figure  3.2.1) 


C24-obstacle  detection  C25  -global  positioning 


FIGURE  3.2.1:  The  five  perception  nodes 

2.2.1  Module  C21  -  Vision  and  Lidar 

Introduction 

This  module  is  used  to  synchronize  the  Cameras  &  Point  cloud  &Lidar  input  and  act  as  a  data 
server  for  data  analyzing  to  be  used  by  the  other  perception  modules. 

Method&Implementation 

Using  the  message  filters  feature  it  synchronizes  the  cameras  and  the  cloud  and  Lidar  data 
together.  The  TF  server  is  used  to  refer  the  data  relative  to  the  pelvis  origin. 


2.2.2  Module  C22  -  Ground  Recognition  and  Mapping 
Introduction 

This  module  receives  3D  Point  Cloud  and  returns  a  matrix  which  represents  an  eagle-eye 
view  of  the  terrain.  This  map  is  divided  into  25cm2squares,  each  holding  data  on  the 
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|  properties  of  the  terrain,  e.g.  the  square  in  the  location  cq ^represents  a  floor,  etc.  The  map 
information  is  used  by  the  path  planning  module  for  mounted  and  dismounted  mobility  tasks. 

Method 

The  module  receives  a  3D  Point  cloud  from  module  C21  which  is  filtered  using  a  VoxelGrid 
Filter.  This  process  will  leave  only  a  relative  amount  of  points  in  order  to  make  the  ground 
mapping  process  more  efficient. 

The  filtered  points  are  now  divided  into  different  groups  of  points  in  a  stage  called 
Segmentation.  Planes  are  then  calculated  from  these  groups  using  RANSAC  methodology. 
These  planes  consist  of  attributes  such  as  slope,  height  and  availability  (floor  or  obstacle)  and 
are  mapped  on  the  matrix  according  to  their  geometric  position. 

For  fast  mapping  the  Lidar  data  is  filtered  and  transformed  to  a  fixed  frame.  This  way  we 
achieve  fast  height  mapping  to  go  along  with  the  plane  detection 

Implementation 

Open  source  code  from  the  PCL  library  was  utilized  in  order  to  perform  data  operations  on 
the  given  point  cloud,  such  as  filtering  and  plane  segmentation  using  RANSAC. 

For  the  filtering  stage,  VoxelGrid  filtering  script  is  employed  and  applied  a  square  threshold 
|  of  5 cm2,  after  filtering  using  this  method  a  significantly  smaller  amount  of  points  is  received. 
Although  fewer  points  are  employed,  it  is  possible  to  achieve  an  impressively  accurate  result. 
For  the  segmentation  step  a  SACSegmentation  script,  that  receives  the  filtered  Point  Cloud 
and  returns  the  planes  which  the  cloud  contains,  was  used. 

After  segmenting  the  points  into  planes,  a  48  x  48  matrix  where  each  cell  represents  a  square 
|  of  2 5 cm2, is  used.  The  size  of  the  matrix  was  defined  based  on  an  assumption  that  the  robot 
can  accurately  see  10m  ahead  and  6 m  to  each  side.  In  the  future  we  also  plan  on  saving  2 m 
of  past  data,  in  order  to  have  the  ability  to  safely  retrace  our  steps,  if  needed.  The  size  of  each 
square  was  defined  by  estimating  the  size  of  the  foot  of  the  DRC  robot,  thus  allowing  or 
disallowing  placing  of  a  foot  in  any  specific  square.  This  module  is  defined  as  a  service,  and 
therefore  data  will  be  provided  to  any  client  upon  request  via  ROS'  publish  and  subscribe 
methods. 

By  using  the  localization  module  we  can  map  the  terrain  from  a  fixed  axis,  this  lowers  the 
need  computation  in  reading  the  map  and  provide  a  better  base  to  detect  inconsistency  in  the 
parsed  data 

Result 

After  implementing  the  code  so  it  calls  upon  all  the  required  ROS  packages,  a  ROS  service 
was  created.  The  service  can  transform  a  snapshot  taken  from  the  DRC  robot's  camera  into 
an  eagle-eye  map  of  the  terrain  on  demand.  Two  examples  can  be  seen  in  the  following 
Figures. 
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(c) 

FIGURE  3.2.2:  (a)  DRC  robot  in  an  empty  world  with  texture,  (b)  RVIZ  of  the  image,  (c) 

eagle-eye  representation 


-j 
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FIGURE  3.2.3:  (a)  DRC  robot  in  an  empty  world  with  texture  and  obstacle,  (b)  RVIZ  of  the 

image,  (c)  eagle-eye  representation 


2.2.3  Module  C23  -  Object  Recognition 
Introduction 

One  of  the  main  tasks  of  the  robot  is  to  successfully  detect  surrounding  objects.  For  example, 
in  order  to  successfully  drive  the  car,  first  the  robot  should  detect  the  car,  reach  the  car  and 
detect  the  steering  wheel.  The  main  purpose  of  this  module  is  to  detect  all  the  tools  that  the 
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robot  will  interact  with,  such  as  cars,  wheels,  doors,  water  plugs  etc. . . 

This  module  can  search  for  multiple  objects,  where  for  each  object,  the  robot  module  will 
return  a  Boolean  number  (1 10)  indicating  whether  it  was  detected  or  not. 

If  the  object  is  detected,  the  module  returns  a  bounding  box  around  the  object. 

Method 

For  this  to  work,  the  module  will  implement  two  algorithms: 

Classifier/  Detector:  Used  for  the  recognition  phase.  The  module  will  assign  a  separate 
classifier  per  object. 

Tracker:  After  the  object  was  detected,  the  module  will  track  the  trajectory  of  the  object  in 
the  next  frames,  it  will  locate  the  object  and  update  whether  the  robot  is  on  the  right  track  or 
not. 

Object  Server:  key  object’s  and  their  location  are  saved  in  the  object  server  for  future 
reference  and  mapping, 

Implementation 

The  TLD  algorithm,  (Track,  Learn  and  Detect)  which  is  a  real-time  algorithm  for  tracking  of 
previously  unseen  objects  in  video  streams,  is  employed.  The  object  of  interest  is  defined  by  a 
bounding  box  in  the  first  frame.  The  result  is  real-time  tracking  of  the  object  that  improves 
over  time. 

The  algorithm  was  trained  using  the  different  orientations  and  scales  of  the  door.  This  way 
algorithm  learns  how  the  object  looks  like,  so  that  with  time  the  object  tracking  and  detecting 
improves.  The  object  server  is  still  under  development. 

2.2.4  Module  C24  -  Obstacle  Detection 
Introduction 

This  module  detects  obstacles  both  under  ground  level  (holes)  and  above  ground  level, 
including  information  on  data  confidence  provided  for  the  position  of  the  detected  obstacle 
and  also  provides  an  eagle-eye  view  of  the  locations  of  the  obstacles  detected. 

Method 

Using  entropy  based  segmentation;  deferent  areas  on  a  given  camera  data  are  labeled,  areas 
not  detected  as  ground  are  immediately  marked  as  obstacles  until  further  confirmation  is 
given.  While  driving  the  C51  node  is  using  the  Lidar  data,  for  fast  detection  of  height  changes 
a  head  of  its  path. 


2.2.5  Module  C25  -  Global  Position 
Introduction 

This  module  provides  the  robot’s  position  in  the  world  using  visual  odometery  and  IMU  data. 

Method 

In  order  to  calculate  the  robot's  location,  a  visual  odometry  node  is  used  to  provide  estimation 
of  the  robot  position  and  accompany  it  with  data  from  the  IMU  for  error  deduction. 

Implementation 

Using  the  LibViso2  position  estimation,  the  odometry  and  IMU  data  are  passed  to  a  Kalman 
filter  to  provide  an  estimation  of  the  robot  movement. 


toil 
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2.3  Decision  Making 

We  implemented  the  ability  to  view  at  real  time  execution  debug  information  (green  denotes 
that  the  task  is  being  executed)  and  history  (Figure  3.3.1,  each  task  can  be  queried  about  past 
values). 


FIGURE  3.3.1:  ROBIL  DESIGNER  WITH  RUNTIME  INFORMATION 


Description  of  the  task  is  now  part  of  the  workspace  environment  allowing  maintenance  and 
update  of  task  description  both  the  programmer  and  the  user  (Figure  3.3.2). 
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FIGURE  3.3.2:  ROBIL  DESIGNER  TASK  DOCUMENTATION 


Mission  and  sub  trees  are  now  collapsible  and  can  be  exported  to  create  and  save  new  sub¬ 
trees  [1-6]  (Figure  3.3.3) 


FIGURE  3.3.3:  ROBIL  DESIGNER  BEHAVIOUR  TREE  FOLDING 

Regarding  task  execution  in  order  to  allow  fast  execution  of  concurrent  tasks  we  modified  the 
action  lib  clients  and  servers  to  better  cope  with  concurrent  goals  (both  for  python  and  C++). 

We  also  have  made  progress  with  our  monitoring  module 

1.  For  monitoring  tasks  were  implemented:  Live  lock,  Stability,  Falling,  and  Timeout: 

1.  Falling  returns  when  robot  has  fallen.  Based  on  information  passed  on  topic 
/C  3  5/falling 
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2.  Live  lock:  returns  if  the  monitored  node  is  taking  longer  than  1  .X  times  slower  than 
expected,  expected  time  is  retrieved  from  each  module. 

3.  Stable:  returns  values  [1-5]  corresponding  to  the  robot's  stability.  Based  on 
information  passed  on  topic  /C35/stability 

4.  Timeout:  returns  if  the  subtree  execution  is  taking  longer  than  X,  where  X  is  given 
as  a  parameter  [6-9]. 

2.  We  support  task- specific  monitoring.  These  are  monitoring  tasks  that  modules  want  to 
maintain  externally.  As  an  example  we  implemented  in  Waypoint  Monitor  which  monitors 
the  avg.  distance  between  waypoints.  If  this  average  distance  is  above  some  threshold,  an 
alert  is  issued. 

3.  We  implemented  progress  monitoring  to  be  used  by  the  HMI.  Based  on  a  comparison  of 
the  progress  made  online  with  the  expected  progress  for  that  time  point  and  the  deadlines  for 
task  completions,  it  gives  recommendations  for  speeding  up  execution. 

Our  current  focus  is  mainly  addressed  to  debugging  and  integration  for  all  the  behavior  trees 
built  by  the  different  teams.  The  future  plans  include: 

1.  Statistics  and  data  collection  on  the  execution  times  of  modules. 

2.  We  implemented  an  early  version  of  a  remote  Designer  with  communication 
optimized  to  the  competition  rules,  allowing  following  execution  remotely  while  using 
network  in  a  smart  manner.  Challenges  arose  from  ROS  underling  communication 
infrastructure,  which  is  not  optimized  to  deal  with  communication  constraints. 

This  module  needs  further  elaboration  and  connection  to  the  HMI. 

3.  We  plan  to  implement  executional  condition  specification  (conditions  that  relate  to  the 
execution  trace  of  the  mission),  and  the  construction  of  behavior  trees  at  real  time,  by 
the  use  of  NLP  driven  methods. 

2.4  Dismounted  Mobility 

Dismounted  mobility  was  led  and  developed  by  the  Technion  group,  including  vehicle 
mounting. 


2.4.1  Locomotion 

In  order  to  complete  Task  2  of  the  Virtual  Robotics  Challenge  we  originally  intended  to 
extend  our  successful  CPG-based,  dynamic  walking  controller,  as  well  as  a  ZMP 
implementation  as  a  fallback  [10-12],  However,  due  to  the  tight  time  frame  of  the  competition 
and  the  ever  changing  simulation  framework  we  decided  to  take  advantage  of  the  Boston 
Dynamics  Interface  (BDI)  controller  supplied  with  the  DRC  environment,  for  biped  walking. 
In  order  to  cross  the  mud-pit  we  developed  a  robust  pentad  mode  of  walking,  which  was  also 
extended  to  crossing  the  hills  and  debris  area. 

Thus,  we  have  developed  four  walking  modes,  to  facilitate  crossing  different  types  of 
terrain.  Two  modes,  the  continuous  dynamic  walk  and  the  pentad  walk  allowed  successful 
completion  of  task-2  as  demonstrated  in  the  video  and  in  subsequent  Figures. 

1.  Continuous-dynamic  walk  (CDW):  supports  biped  locomotion  to  the  target,  or  a 
sequence  of  via-points.  It  automatically  generates  positions  for  foot-placement  and 
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sends  the  required  foot  placement  sequence  to  the  Boston  Dynamics  dynamic  walking 
module.  It  is  adequate  for  walking  in  the  pen,  walking  on  flat  terrain  and  crossing  the 
debris  area.  In  the  actual  competition  it  was  used  to  exit  the  pen  and  to  get  to  the 
vicinity  of  the  car  or  table. 

2.  Discrete-foot-placement  walk  (DFPW):  supports  biped  locomotion,  either  quasi  - 
statically  or  dynamically,  based  on  an  externally  specified  vector  of  foot  placements. 
It  could  be  used  to  cross  the  hills  quasi-statically,  as  shown  in  Figure  3.4.1,  provided 
that  proper  foot-placements  are  specified  [13-16].  It  was  not  used  in  the  actual 
competition. 


FIGURE  3.4.1:  Hill  crossing  using  dynamic  foot-placement  walk  (DFPW).  The  robot  is 
able  to  walk  slowly  by  stepping  on  specific  locations  were  the  robot  can  remain  stable. 

3.  Pentad-walk  (PW):  We  developed  a  pentad  gait  framework  that  included  a  safe  sit- 
down  sequence,  forward  pentad-walking  to  enter  the  mud  pit  and  backwards  pentad¬ 
walking  to  exit  the  mud  pit,  as  depicted  in  Figure  3.4.2,  and  in  the  above  movie. 
Furthermore,  two  different  motion  sequences  were  designed  to  turn  in  place  under  low 
and  normal  ground  friction  conditions. 

The  PW  uses  both  hands  and  feet  to  support  the  weight  of  the  robot,  facing  up. 
The  pelvis  is  then  lifted,  moved  forward/backwards  and  placed  back  on  the  ground. 
This  is  followed  by  a  forward/backward  motion  of  the  legs  and  hands.  This 
locomotion  method  is  more  stable  than  the  bipedal  methods  since  the  COM  is  closer 
to  the  ground  and  the  support  polygon  is  much  larger.  The  basic  mode  can  support 
motion  perpendicular  to  the  terrain’s  contour  lines,  and  was  developed  for  entering, 
crossing  and  exiting  the  mud-pit. 
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FIGURE  3.4.2 :Mud-pit  crossing  using  pentad  walk  (PW),  see  movie:  (A)  Approach  mud, 
(B)  sit  down,  (C)  enter  mud  walking  forward,  (D)  turn  180  degrees  inside  mud  pit,  (E) 
exit  mud  walking  backwards,  (F)  cross  mud  gate 

Our  PW  framework  was  also  very  successful  at  crossing  the  hills  section  and 
debris  section  of  the  task,  especially  by  moving  backwards,  as  shown  in  Figure  3.4.3, 
and  3.4.4,  respectively,  and  in  the  above  movie.  Crossing  the  hills  and  the  debris  area 
presented  a  new  challenge  since  it  may  cause  the  robot  to  fall.  Hence  we  enhanced 
the  basic  algorithm  in  two  ways:  (i)  feedback  and  compliance  were  added  to  minimize 
falling,  and  (ii)  an  algorithm  for  fall  recovery  was  developed.  Special  motion 
sequences  were  tailored  to  recover  from  left,  right,  forward  and  backwards  tipping  [8- 
10].  A  higher  level  controller  was  designed  to  autonomously  walk  towards  points 
along  a  path,  correcting  for  orientation  drift  and  recovering  from  falls.  The  robot  can 
autonomously  detect  a  failure  to  advance  and  locally  adjust  the  path  to  circumvent  the 
problematic  area. 

Using  our  pentad-walking  framework  ATLAS  is  able  to  cross  from  gate  to  gate 
autonomously,  using  only  a  2-point  path,  even  with  a  noisy  odometry,  as  long  as  the 
cumulative  errors  are  smaller  than  half  the  gate’s  width.  In  the  actual  competition 
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pentad-walk  was  used  successfully  to  cross  the  mud  and  the  hills  and  to  start  crossing 
the  debris,  as  shown  in  the  following  clip 


FIGURE  3.4.3:  Hill  crossing  using  pentad- walk  (PW,  see  movie). 

The  robot  crosses  the  hill  section  autonomously,  walking  towards  the  center  of  the  next 
gate.  The  robot  is  able  to  recover  from  tipping  in  any  direction  and  can  locally  modify  its 
path  if  it  gets  stuck  [16-21], 


FIGURE  3.4.4:  Debris  crossing  using  pentad- walk  (PW,  see  movie).  The  robot  crosses  the 
hill  section  autonomously,  walking  towards  the  center  of  the  next  gate.  The  robot  is  able 
to  recover  from  tipping  in  any  direction  and  can  locally  modify  its  path  if  it  gets  stuck. 


4.  Translation:  supports  continuous  dynamic  translation  relative  to  the  current  robot 
position.  This  mode  was  developed  to  facilitate  getting  near  the  car  or  table  and  to 
allow  making  short  steps. 

In  addition  the  dismounted  mobility  included  capabilities  to: 

5.  Sit  down  and  stand  up  to  support  transition  from  biped  to  pentad  locomotion. 
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6.  Turning  in  either  pentad  or  biped.  Turning  in  pentad-state  was  used  to  turn  in  the 
middle  of  the  pit  to  allow  the  robot  to  exit  stably  by  walking  backward. 

2.4.2  Vehicle  Mounting  and  Dismounting 

Our  first  approach  for  getting  into  the  car  involved  grasping  the  car's  frame  and  then  stepping 
into  the  car  with  the  right  leg.  This  approach  wasn't  robust  and  while  the  robot  was  able  to 
reach  the  seat's  height,  we  were  unable  to  get  it  to  enter  the  car. 

Since  we  lacked  autonomous  perception  and  grasping  capabilities  to  perform  the  task  in  this 
manner,  we  decided  to  go  with  an  open-loop  motion,  generated  manually.  We  have  attempted 
motions  that  jump  in  and  out  of  the  vehicle,  fall  in  or  out,  enter/exit  from  the  sides  by 
stepping  or  sliding,  or  crawling.  Finally,  thinking  outside  box,  wrap  the  arms  around  the  car's 
frame  (on  the  passenger  side),  lift  the  robot's  legs  and  swing  into  the  car,  as  depicted  in  Figure 
3.4.5,  and  demonstrated  in  the  video.  We  ensured  that  the  robot  laid  flat  on  the  seat  while 
getting  the  arms  inside  the  car  and  then  performed  a  sit-up  motion  that  ends  with  the  robot 
facing  the  steering  wheel.  Due  to  friction  issues  between  ATLAS  and  the  car  we  added  some 
arm  and  leg  motions  that  greatly  improve  the  repeatability  of  the  motion  sequence.  We  added 
an  optional  motion  to  press  the  gas  pedal  at  the  end  of  the  sequence  in  order  to  secure 
additional  points  for  crossing  the  first  gate  with  the  car.  However,  getting  into  the  car,  the 
robot  would  randomly  presses  the  reverse  button  causing  unexpected  results. 


FIGURE  3.4.5:  Swing  into  car  motion  sequence.  (A)  Wrap  arms  around  frame,  (B)  lift  legs, 
(C)  swing  into  car,  (D)  release  frame  to  fall  on  seat,  (E)  sit-up  and  (F)  rotate  to  face  the 
steering-wheel. 


3.4.3  Software  Design 

The  intent  of  the  software  design  chosen  for  the  Dismounted  Mobility  module  was  to  allow 
for  several  different  walking  modes  to  be  interchangeable  while  keeping  the  same  behavioral 
mechanism  through  which  the  module  interacts  with  other  modules. 
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FIGURE  3.4.6:  Dynamic  Locomotion  class  diagram  for  Dismounted  Mobility 


As  seen  in  the  class  diagram,  Figure  3.4.6,  a  class  called  DynamicLocomotion  is  a  RobilTask 
which,  when  a  task  is  provoked,  runs  a  predefined  sequence  consisting  of  getting  ready  to 
walk,  walking,  stopping  and  sending  out  the  result  of  the  walking  task.  The 
DynamicLocomotion  has  a  Walker  class  instance,  which  encapsulates  the  WalkingMode  and 
WalkingModeChooserc lasses.  The  Walker  is  responsible  for  the  correct  performance  of  the 
walking  sequence  by  the  right  WalkingMode.  If  the  WalkingMode  isn't  fit  to  perform,  the 
WalkingModeChooser  is  used  to  replace  the  inadequate  WalkingMode  with  a  more 
appropriate  one. 

An  example  for  a  need  to  replace  one  WalkingMode  with  another  happens  when  a 
Discrete-foot-placement  path  has  changing  heights  in  it,  which  requires  a  change  from 
dynamic  walking  mode  to  quasi-static,  without  interrupting  the  task  sequence. 

In  order  to  sustain  a  uniform  behavioral  mechanism  independent  of  actual  walking 
modes,  while  allowing  higher  levels  of  decision  making  to  have  control  over  the  actual 
walking  modes  used,  a  Factory  design  pattern  was  introduced  in  the  form  of  the  WalkerNode. 
The  WalkerNode  is  responsible  for  the  construction  of  the  DynamicLocomotion  class  and  the 
construction  of  a  concrete  WalkingModeChooser  to  accompany  the  DynamicLocomotion.  The 
higher  level  decision  making  module  can  thus  control  which  WalkingMode  instances  are 
available  for  the  WalkingModeChooser  and  accordingly  override  the  control  over  which 
walking  mode  is  used  during  the  performance  of  the  task. 
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The  WalkingMode  is  an  abstract  class  which  is  responsible  for  the  actual  walking  to  take 
place,  as  shown  for  example  in  Figure  3.4.7.  It  has  a  PathMonitor  to  manage  the  progress 
over  a  given  path  and  a  WalkingModeStateMachine  which  manages  the  different  phases  of 
the  walking  task 


FIGURE  3.4.7:  An  actual  Walking  Mode  and  its  inheritance  relationship  with  the  abstract 
Walking  Mode  class 


In  the  class  diagram  shown  in  Figure  3.4.7,  the  Continuous  Dynamic  Walking  Mode 
(CDWM)  class  architecture  is  shown  as  an  example  of  how  the  strategy  design  pattern  was 
used.  Using  this  pattern,  all  walking  modes  were  easily  fitted  into  the  general  design.  The 
WalkingModeChooser  class  is  an  aggregation  class  designed  to  hold  a  group  of  WalkingMode 
strategy  instances  and  choose  among  them  which  is  best  suited  for  the  upcoming  path. 

As  shown  in  Figure  3.4.8,  the  Walker  class  has  a  WalkingModeChooser  which  in  turn 
has  a  function  that  returns  a  boolean  answer  to  the  question  "Is  the  current  walking  mode 
appropriate?".  The  WalkingMode  class  has  a  Fitness()  function  which  returns  true  if  the 
WalkingMode  is  fit  to  continue  or  false  otherwise.  The  WalkingModeChooser  may  have 
several  Walking  Modes,  and  a  key  for  knowing  how  desirable  each  Walking  Mode  is  (set  by 


fPotail 
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higher  levels  through  the  WalkerNode  factory).  If  the  current  mode  is  unfit,  or  if  there  is  a 
more  desirable  mode  which  is  fit,  the  WalkingModeChooser  indicates  that  the  current  mode  is 
inappropriate  and  recommends  on  a  different  walking  mode  instead. 

Through  the  use  of  the  strategy  pattern,  the  Walker  may  change  from  one  walking  mode  to 
another  without  any  awareness  as  to  which  mode  is  which,  the  whole  mechanism  is  facilitated 
by  the  WalkingModeChooser  indicating  that  a  change  is  needed,  and  supplying  the  Walker 
class  with  a  replacement  WalkingMode. 


FIGURE  3.4.8:  The  different  Walking  Modes  available  to  the  Walking  Mode  Chooser  and 
their  relationships 


2.5  Mounted  Mobility 

2.5.1  Introduction 

One  of  the  tasks  anointed  by  DARPA  is  driving  a  utility  vehicle.  The  utility  vehicle  is  non- 
holonomic,  i.e.  the  turning  abilities  of  the  vehicle  is  limited  and  the  vehicle  can't  turn  in  place. 
In  order  to  drive  the  non-holonomic  vehicle,  the  robot  will  build  a  path,  and  execute  the 
driving  process  based  on  the  chosen  trajectory  [22,  23],  Thus,  we  will  distinguish  between 
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two  (2)  different  processes  related  to  this  task.  The  first,  the  robot  will  create  a  trajectory  on 
the  fly.  This  procedure  will  be  executed  by  “Decision  Making”,  in  module  C31  -  “Path 
Planning”.  The  output  of  C31  is  a  series  of  way  points  that  will  be  distributed  non- 
homogeneously  along  the  trajectory,  according  to  path  condition.  The  second  process  is  to 
drive  the  car  through  the  way  points  given  by  C31.  The  car  is  driven  using  a  fuzzy  logic 
controller  (FLC),  alongside  a  reactive  controller  that  will  aid  in  the  obstacle  avoidance 
process. 

In  this  chapter  we  will  describe  the  fuzzy  logic  controller,  communication  with  the  different 
modules  and  show  the  results  achieved. 

2.5.2  Assumptions 

We  assume  that  Atlas  is  situated  in  the  vehicle,  in  an  upright  position  in  front  of  the  steering 
wheel  and  in  a  static  position.  One  of  our  main  issues  was  keeping  Atlas  situated  in  the  seat. 
Due  to  the  low  friction  of  the  seat,  Atlas  always  slipped  and  fell  off  the  seat. 

Another  assumption  is  that  all  of  the  modules  are  running  and  are  integrated  with  each  other, 
i.e.,  decision  making  is  integrated  with  perception  and  the  global  position  modules;  dexterity 
is  integrated  with  the  object  recognition.  We  also  assume  that  the  global  position  module 
(xcar,  Year)  and  the  IMU  (0car)  are  operating  with  reasonable  error  between  way  points. 

2.5.3  Methods 

Fuzzv  Logic  Controller 

We  will  create  a  negative  feedback  control  loop  using  the  location  (xd,yd)  of  the  way  point 
we  wish  to  arrive  at,  and  the  location,  orientation  and  orientation  rate  of  the  car 
(xcar>  Year*  6Car-  0Car)>  as  described  in  0,  throughout  the  simulation. 
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FIGURE  3.5.1:  Car  and  Way  Point  sketch 

With  these  five  (5)  variables  (xd,  yd,  xcar,  ycar,  0car)  we  can  calculate  two  (2)  new  variables: 
Distance  from  the  car  to  way  point  (de).  The  distance  is  defined  as  follows: 


(3.5.1)  de  —  y]  (Xd  "year )2 
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Orientation  error  (0e)  -  Defined  as  the  difference  between  the  orientation  of  the  car  and  the 
slope  angle  of  the  car  and  the  way  point.  The  orientation  error  is  calculated  as  follows: 

(3.5.2)  ee=tan-1^^-0c„ 

Ad  Acar 

These  two  (2)  new  variables  along  with  the  orientation  rate  (0car)  will  be  the  input  variables 
of  the  fuzzy  logic  controller  (FLC)  in  order  to  control  the  vehicle.  The  output  of  the  FLC  will 
be  the  desired  velocity  and  the  desired  angle  of  the  steering  wheel  [24-28]. 

Fuzzy  Logic  Membership  functions 

In  order  to  turn  the  crisp  inputs  into  fuzzy  variables,  we  will  define  the  following  normalized 
(i.e.  values  between  0  and  1),  membership  functions: 


2.5.3. 1  Orientation  error  membership  function 

For  the  orientation  error  membership  function,  we  will  assign  five  (5)  linguistic  variables:  Big 
Negative  (BN);  Small  Negative  (SN);  Zero  (Z);  Small  Positive  (SP);  &  Big  Positive  (BP).  Let 
us  denote  the  crisp  input  of  the  orientation  error  as  0e,  and  define  the  mathematical  relation  of 
the  linguistic  variables  with  0e  as  follows: 
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The  corresponding  membership  function  schematic  is  described  in  0. 


-0.4  -0.3  -0.2  -0.1  0  0.1  0.2  0.3  0.4  0.5 

input  variable  "Orienatation* 


FIGURE  3.5.2:  MATLAB  orienation  error  membership  function  schematic. 
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2. 5.3.2  Change  in  orientation  error  membership  function 

For  the  orientation  error  membership  function,  we  will  assign  five  (5)  linguistic  variables:  Very 
Negative  (N);  Negative  (N);  Zero  (Z);  Positive  (P);  &  Very  Positive  (P).  Let  us  denote  the  crisp  input  of 
the  change  in  orientation  error  as  0e,  and  define  the  mathematical  relation  of  the  linguistic  variables 
with  0e  as  follows: 
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The  corresponding  membership  function  schematic  is  described  in  0. 


-0.4  -0.3  -0.2  -0.1  0  0.1  0.2  0.3 

input  variable  'dOrientation' 


FIGURE  3.5.3:  MATLAB  'change  in  Orientation  error'  membership  function  schematic. 


2. 5.3.3  Distance  membership  function 

For  the  distance  membership  function,  we  will  assign  three  (3)  linguistic  variables:  Close, 
Far,  and  very  Far.  Let  us  denote  the  crisp  input  of  the  distance  as  de,  and  define  the 
mathematical  relation  of  the  linguistic  variables  with  de  as  follows: 
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The  corresponding  membership  function  schematic  is  described  in  0. 


0.2  <  de 
0.2  <  de  <  0.4 
0.4  <  de  <  1 
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0  0.05  0.1  0.15  0.2  0.25  0.3  0.35  0.4  0.45  0.5 

input  variable  "Distance* 


FIGURE  3.5.4:  MATLAB  'Distance'  membership  function  schematic. 

2. 5.3.4  Steering  angle  membership  function 

Before  we  assign  the  membership  function,  let  us  note  that  we  will  grip  the  steering  wheel 
with  one  hand  in  order  to  avoid  any  complications  with  changing  hands  while  driving.  For 
this  matter  we  will  grip  the  steering  wheel  from  the  highest  point  and  will  denote  this  angle  as 
0[rad].  We  will  now  turn  the  steering  wheel  left  and  right  up  too  -  [rad]  to  each  direction. 

This  will  make  the  turning  process  simple  and  effective  as  it  answers  for  most  of  the  turning 
operations  required. 


FIGURE  3.5.5:  Hand  wheel  grip  at  angle  0. 


For  the  steering  wheel  angle  membership  function,  we  will  assign  also  five  (5)  linguistic 
variables:  Hard  Left  (HL);  Soft  Left  (L);  Straight  (ST);  Soft  Right  (R);  &  Hard  Right  (HR). 
The  steering  wheel  angle  membership  function  schematic  is  shown  in  0. 


Membership  function  plots 


FIGURE  3.5.6:  MATLAB  ’Steering  wheel  angle’  output  membership  function  schematic. 
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2. 5.3.5  Velocity  membership  function 

For  the  velocity  membership  function,  we  will  assign  three  (3)  linguistic  variables:  Slow,  Fast,  and 
very  Fast. 

The  corresponding  membership  function  schematic  is  described  in  0. 


0  0.5  1  1.5 

output  variable  ’Unear-Speed" 


FIGURE  3.5.6:  MATLAB  'Speed'  output  membership  function  schematic. 

2. 5.3. 6  Inference  engine 

The  inference  engine  is  constructed  of  an  "if  distance  is  de  and  Orientation  is  0e  and  Change  in 

Orientation  is  0e,  then  velocity  is  v  and  the  steering  wheel  angle  is  @ "  statements.  In  order  to 
receive  a  crisp  output  value  we  will  use  the  "Center  of  mass"  technique. 

The  inference  engine  correlation  between  the  orientation  and  the  change  in  orientation  is  described 
in  Table  3.5.1  below. 


Table  3.5.1  -  Inference  table  between  6e  and  6e 
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Obstacle  maneuvering 

One  of  the  benefits  of  using  the  fuzzy  logic  controller,  is  that  there  is  an  easy  way  to  implement 
obstacle  maneuvering  by  changing  the  weight  of  each  rule  in  case  of  obstacle  detection. 
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The  detection  of  the  obstacles  is  done  using  the  LiDAR.  The  LiDAR  provides  a  good  mapping  of  the 
surrounding  with  minimum  of  calculation,  as  opposed  to  visual  point  cloud  from  the  cameras. 


FIGURE  3.5.7:  On  the  left  2D  grid  of  the  obstacles  seen  on  the  right. 

The  points  are  collected  and  per  frame  a  new  weight  is  calculated  for  each  one  of  the  orientation 
membership  functions.  The  weight  is  calculated  for  each  membership  function  depending  on  the 
proximity  to  the  vehicle  and  amount  of  obstacle  points  in  the  region,  as  the  proximity  increases  the 
weight  decreases  and  the  other  membership  function  will  have  greater  weight  and  the  vehicle  will 
diverge  from  the  obstacle. 

2.5.3.7  Procedures 

2.5.3.7.1  ROS  'actionlib' 

The  high-level  driving  task  is  written  as  a  ROS  actionlib.  Actionlib  is  used  for  monitoring  and  the 
ability  to  stop  the  script  after  it  is  executed,  as  opposed  to  ROS  service-client. 

2.5.3.7.2  Receiving  way  points  from  path  planning 

The  first  operation  is  to  collect  the  way  points  from  the  path  planner,  these  points  are  sent  via  a  ROS 
"topic",  and  recollected  each  time  50%  of  the  points  in  the  buffer  are  reached.  These  points  act  as  a 
guide  line,  and  it  is  not  mandatory  to  pass  each  one  of  them,  or  even  to  pass  them  very  close. 

2. 5. 3. 7.3  Calculating  the  desired  speed  and  steering  wheel  angle 

Once  we  have  the  way  points,  we  can  start  the  logical  driving  process.  The  way  point  is  used  to  send 
data  to  the  FLC,  along  with  the  weight  factor  calculated  using  the  LiDAR.  As  mentioned  before,  the 
output  of  the  FLC  is  the  desired  speed  and  steering  wheel  angle. 

2.5.3.7.4  Sending  the  calculated  speed  and  steering  angle 

Dexterity  will  collect  the  values  sent  to  them,  via  a  ROS  "service"  and  perform  the  actual  movement 
by  Atlas. 


2.5.4  Results 

After  egression  and  first  car  initialization  (gripping  the  steering  wheel,  moving  gear  to  forward, 
placing  the  feet  on  the  pedals  and  releasing  the  hand  brake),  the  ROS  'actionlib1  is  started.  Atlas 
starts  driving  to  the  received  way  points  until  it  reaches  the  last  gate.  There  it  stops  with  a  'success' 
result. 
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The  driving  task  is  performed  on  an  8  meter  wide  road  and  consisted  of  a  starting  gate,  end  gate  and 
obstacles.  0  shows  the  route  driven  by  Atlas.  The  route  is  approximately  270  meters  long. 


FIGURE  3.5.8:  The  route  driven  by  Atlas. 


FIGURE  3.5.9:  Atlas'  path  for  driving  task. 

Atlas'  path  is  recorded  and  shown  in  0.  You  can  view  the  results  in  a  movie  uploaded  to  YouTube.  In 
the  movie  you  can  see  Atlas  handling  the  vehicle  in  the  three  stages  of  driving.  In  the  first  stage,  the 
initialization  stage,  Atlas  releases  the  handbrake,  and  grasps  the  hand  wheel.  In  the  second  stage, 
the  driving  stage,  Atlas  drives  the  utility  car,  while  maneuvering  obstacles,  from  the  starting  point 
until  the  end  point,  which  is  the  last  gate.  The  third  stage  (which  is  not  shown  in  the  movie),  the 
finalization  stage,  Atlas  pulls  the  handbrake,  and  releases  the  hand  wheel. 
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2.5.5  Discussion 

The  excellent  way  point  tracking  can  be  seen  in  0.  In  addition,  one  can  see  the  obstacle  avoidance 
performed  before  the  barrels  (30  and  60  meters  from  the  starting  gate). 

The  fuzzy  logic  controller  is  an  excellent  tool  for  the  driving  task  since  the  car's  parameters  are 
unknown  to  Atlas,  and  yet  the  FLC  doesn't  need  these  parameters  in  order  to  operate  with  good 
performance.  In  addition,  the  FLC's  membership  functions  and  values  can  be  tuned  very  easily  and 
can  even  be  adaptive  in  some  more  complicated  algorithms. 

2.5.6  Conclusions 

A  fuzzy  logic  controller  is  a  very  simple,  yet  robust,  way  to  design  control  systems  using  human 
experience  over  mathematical  modeling.  Moreover,  it  is  a  good  way  for  bypassing  linearization  for 
nonlinear  systems,  thus  making  it  very  suitable  for  this  type  of  control  loops  since  the  car's 
parameters  are  unknown.  In  addition,  an  obstacle  maneuvering  algorithm  is  quite  easy  to 
implement,  a  task  which  is  hard,  or  even  impossible,  to  implement  in  classic  control  systems. 

2.6  Dexterity 

2.6.1  Dexterity  problem  description 

•  Manipulate  the  steering  wheel,  car  ignition,  parking  and  driving  modes. 

•  Hose/cable  connection  task. 

•  Manipulate  Driller. 

2.6.2  Task:  Single  arm  motion 
Introduction: 

This  module  receives  a  position  and  orientation  destination  vector  and  returns  a  joint 
angles  vector  which  bring  the  arm  to  the  desired  position. 

Method: 


A  single  arm  motion  is  attained  as  combination  of  successive  changes  of  the  arms  six 
joints.  In  order  to  perform  the  motion,  a  relation  between  position/orientation  to  the  arm 
joints  angles  needs  to  be  set.  The  solution  can  be  attained  by  employing  Inverse 
Kinematics  approach  which  finds  a  specific  solution  (analytic  or  numeric)  for  the  6 
equations  created  by  Forward  Kinematics  representation  (3.6.1). 


F.K : 


roll 


y 

pitch 


yaw  J 


=  /(<?!  02  03  04  05  06 ) 


1.K : 


(3.6.1) 


V 

02 

03 


^5 

V<h) 


=r 


roll  pitch  yaw  / 


The  resulted  6  joints  angles  obtained  is  input  to  the  joints  position  PID  controllers,  so  the 
arm  moves  directly  to  its  goal. 


34 


Distribution  A:  Approved  for  public  release;  distribution  is  unlimited. 


DARPA  —  Tactical  Technology  Office  (TTO) 

AWARD  NO.  FA8655-12-1-2143 

DARPA-BAA- 12-39  -  “DARPA  Robotics  Challenge” 

Project  Name:  ROBIL 

2.6.3  Single  arm  motion  and  numerical  solution 

Solving  the  inverse  kinematics  problem  of  the  hand  relative  to  the  shoulder  gives  us  six 
equations  for  positions  and  orientation.  The  solution  gives  us  angles  of  the  six  joints  of 
the  arm.  This  means  that  we  got  6  nonlinear  equation  and  6  parameters  to  find. 

We  made  an  inverse  kinematics  solver  for  the  hand  relatively  to  the  shoulder.  We  changed 
the  solution  so  it  considers  the  orientation  inside  its  calculation.  To  generate  a  solution  a 
Jacobian  of  the  forward  kinematics  has  been  calculated  to  describe  infinitesimally 
velocity.  Integrating  the  velocity  results  an  infinitesimally  position.  This  numeric  method 
applied  to  create  a  velocity  and  position  profiles  so  at  the  end  it  gives  the  total  Inverse 
Kinematics  solution. 


(3.6.2)  Qam,[i+ 1]  =  J J[i] '  {Y^baI } dt 


Initial  position  X  motion  Z  motion 


Y  motion  Roll  motion  Pitch  motion 


FIGURE  3.6.1:  Robils  Hand  motion  in  x,y,z, roll, pitch, yaw  directions. 
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For  grasping  object  (like  in  the  driller  qualification)  a  power  grasp  is  obtained  and  is  being 
implemented  such  a  way  it  close  the  fingers  from  an  initial  position  until  the  force  sensor 
stops  the  process. 

2.6.4  Single  Arm  motion  Analytic  method 

2.6.4.1  Forward  Kinematics 

The  last  6  degrees  of  Freedom  from  shoulder  to  hand  form  the  arm  movement  without  the 
fingers.  The  placement  of  the  hand  is  driven  also  from  the  3  back  joints.  The  base  position  is 
the  pelvis  position.We  number  the  joints  from  the  pelvis  to  the  hand  ql-q9. 


Transformation  Matrices 

2.6.4.1.1  Back  joints 

Back  links  are  driven  by  joints 

•  Lower  Torso  (ltorso)  is  turned  around  Z  axis  by  the  back_lbz  joint.(ql) 

•  Middle  Torso  (mtorso)  is  turned  around  Y  axis  by  the  back_mby  joint. (q2) 

•  Upper  Torso  (utorso)  is  turned  around  X  axis  by  the  back_ubx  joint.(q3) 

The  Transformation  matrices  are: 


C0, 

-S0J 

0 

'  C02 

0 

S02 

0 

1 

0 

0 

0  ' 

S0, 

C0, 

0 

0 

't7= 

0 

1 

0 

0 

%= 

0 

ce3 

-se  3 

0 

0 

0 

1 

0 

— S02 

0 

ce2 

d* 

0 

S03 

C03 

0 

0 

0 

1 

i 

0 

0 

0 

1 

0 

0 

0 

1 

(3.6.3) 

These  transformation  matrices  are  used  for  both  arms  equally. 

2. 6.4.1. 2  Arm  Joints 

We  have  6  joints  in  the  arm. 

•  Upper  Shoulder  (clav)  is  turned  around  2  axis  Y  and  Z  by  the  right/  left  arm_usy 
joint  (q4  right/left) 

•  Shoulder  (scap)  is  turned  around  X  axis  by  the  right/left  arm_shy  joint  (q5  right/left) 

•  Upper  Arm  (uram)  is  turned  around  Y  axis  by  the  right/left  arm_ely  (q6  right/left) 

•  Lower  Arm  (larm)  is  turned  around  X  axis  by  the  right/left  arm_elx  (q7  right/left) 

•  Forward  Arm  (farm)  is  turned  around  Y  axis  by  the  right/left  arm_umy  (q8  right/left) 

•  Hand  (hand)  is  turned  around  X  axis  by  the  right/left  arm_mwx  (q9  right/left) 

The  Transformation  Matrix  of  the  Upper  Shoulder  (q4)  is  the  most  complex: 
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C04 

-rj4se4 

ry4S04 

dX4 

rz4  $  04 

ry42ve4+C04 

ry4rz4V04 

dy4 

-ry4S04 

ry4rz4Ve4 

rz42  V04+C04 

d  z4 

0 

0 

0 

1 

(3.6.4) 

(3.6.5)  Vdi  =  l-  Cos(Gi) 

Transformation  matrices  of  the  rest  of  the  joints  (q5-q9) 
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(3.6.6) 


Here  is  the  table  of  constants  for  the  right  arm: 


Axes 

System 

number 

Link  name 

Joints 

Joints 

angle 

Axes  constants 

1 

itorso 

baekjbz 

0! 

<1^=- 0,0 125 

2 

mtorso 

back_mby 

02 

<^=0.09 

3 

u  torso 

back_ubx 

03 

<^=0.05 

4 

r_clav 

r_arm_usy 

04 

r^=- 0.5,  ri4= 0.866,  dx4  -  0.024,  d^ =■ -  0.22 1 ,  dl4 = 0.289 

5 

r_scap 

r_arm_shx 

03 

(^=-0.075,(1^=0.036 

6 

rjiram 

r_arm_ely 

06 

<^=-0.185 

7 

rjarm 

r_arm_e)x 

07 

dJ,7=-0.121,dl7=0.013 

8 

r_farm 

r_arm_umy 

0B 

dvfl=-0.188,drf=-  0.013 

9 

r_hand 

r_arm_mwx 

03 

d^ =-0.058 

2.6A.2  Inverse  Kinematics 

If  We  Know  the  3  back  joints  position,  we  will  calculate  the  rest  6  joint  position  by  the  give 
hand  position  and  orientation. 

2.6.4.2.1  The  q4  joint  problem 
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The  Upper  Shoulder  (q4)  joint’s  Transformation  is  much  more  complicated  than  the  rest  of 
the  joint’s  Transformation  (see  Forward  Kinematics).  While  in  the  Forward  Kinematics  it’s 
just  a  little  more  complex  Matrix  multiply,  the  Inverse  Kinematics  Solution  turn  to  be 
impossibly  complex.  Over  Idea  was  to  scan  the  q4  joint,  then  calculate  the  rest  5  joints 
analytically. 

2.6A.2.2  Solving  q5-q9  analytically 
The  Transformation  matrix  from  q4  to  q9: 


(3.6.7) 

By  inverse  multiply  from  both  sides  we  can  get: 


m  4/ti  5  rrt  6  rp  7  m  8  m  T  /l  S  Cl  "1 

1  9~  1  5 1  6*  1  7*  1  8*  Lq  0  0  1 


(3.6.8) 

The  multiplication  result: 


i/rt  6m  7  rp  _  4m — 1  r  /I  S  Cl  ^  ~\  1 

e'  ?•  8-  5  Iq  0  0  ^  9 


c6c8-c7s6s8  s6s7  c6s8+ c7c8s6  z7-s6+  z8c7s6+  y8s6-s7 
r  s7s8  c7  —c8s7  y6+  y7+  y8-c7  —  z8s7  -i 

—c8s6—c6c7  s8  c6-s7  c6c7c8—s6  s8  z7c6+  z8c6-c7+  y8c6  s7 
0  0  0  1 
nx  sxc9  —  axs9  axc9+  sxs9 

I nyc5+  nzs5  c9-(sy-c5+  sz-s5)  —  s9-(ay-c5+  az-s5)  s9-(sy-c5+  sz-s5)+  c9-{ay-c5+  az-s5) 
nz  c5-ny-s5  c9-(sz-c5-sy-s5)-s9-(az-c5-ay-s5 )  s9-(sz-c5-sy-s5)+  c9-(az-c5-ay-s5 ) 
0  0  0 

px+  axy9s9  —  sxy9c9 

py-c5—  y5c5+  pzs5—  z5s5+  y9s9-(ayc5+  az-s5)—y9-c9-(sy-c5+  sz-s5)w^\ 
pzc5-z5-c5—  pys5+  y5-s5+  y9-s9-{az-c5—ay-s5)-y9-c9-{sz-c5—sy-s5) 

(3.6.9)  1 

If  we  will  use  column  2  and  4  we  will  get  6  equations  that  don’t  involve  q8 


For  column  2(orientation): 


(3.6.10) 


'  s6  s7] 

i 

sx-c9-axs9 

c7 

_ 

c9-(sy-c5+  sz-s5)— s9-(ay-c5+  az-s5) 

,  c6-s7j 

c9-(sz-c5-sy-s5)— s9-(az-c5-Qy-s5)j 

(2) 


For  column  4(position): 
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z7-$6  +  z8c7s6+  y8s6- s7 
y6+  y7+  y8c7-z8s7  = 

z7c6  +  z8-c6c7+  y8-c6s7i 

px+  axy9-  s9—sxy9c9 
py  c5—y5  c5+  pz  s5- z5- s5+  y9-s9-{ay  c5+  az  s5)— y9-c9-(sy-c5+  sz-s5)  (3) 
pzc5-z5c5—  py-s5+  y5-s5+  y9-s9-(azc5— aysS)— y9-c9-(sz-c5  — sy-s5)l 


(3.6.11) 

Using  (1)  and  (2)  we  can  write  (3)  as: 


(3.6.12) 


z7s6+  z8c7-s6+  y8s6s7 
y6+  y7+  y8c7—  z8s7 
z7-c6+  z8-c6-c7+  y8c6s7 


px  —  s6-s7-y9 

c5-{py-y5)—c7-y9+  s5-[pz-z5) 
c5-(  pz—  z5)— s5-(  py-y5)-c6-s7-y9 


(4) 


After  collecting  arguments  and  moving  expressions  involving  q6  and  q7  from  right  side  to 
left,  we  get: 


(3.6.13) 


1  s6-(z7+  c7-z8+  s7-y89) 

px 

y67  +  c7y89  -  s7 •  z8 

~ 

c5 ■  py5+  s5 ■  pz5 

t  c6-(z7+  c7-z8+  s7-y89)j 

,  c5-pz5-s5-py5 

(5) 


Where: 


(3.6.14)  y67=y6+  y7  ,y89=y8+  y9 ,  py5  =  py—y5  ,  pz5  =  pz—  z5 

If  we  square  the  middle  expression  (5y)  we  get: 

c72-y892—2-c7-s7-y89  z8+  2-c7-y67- y89+  s72- z82-2-s7-y67  z8+  y672  = 
(2  6  ^  c52- py52+  2 -c5-s5- py5- pz5+  s52- pz52{ 6) 

If  we  square  and  add  (5x)  and  (5z)  we  get: 

c72-z82+  2-c7 -s7-y89- z8+  2-c7- z7z8+  s72-y892+  2-y89s7-z7+  z72  = 


(3.6.16) 

Adding  (6)  and  (7): 


c52- pz52-2-c5-S5- py5- pz5+  s52-py52+  px2(  7) 


y672+  2- c7  ■  y67  •  y89 -2- s7  ■  y67  ■  z8+  2-s7-y89z7+  y892+  z72+  2-c7- z7z8+  z82  = 

(3.6.17)  px2+  py52+  pz52{  8) 

Collecting  and  moving  all  arguments  to  left  side: 

c7-2-{y67-y89+  z7-z8)+  s7-2-{y89-z7-y67-z8)- px2- pyS2- pzS2+  y672+  yS92+  z72+  z8 2 

(3.6.18)  =0^ 

We  can  represent  the  equation  as: 


(3.6.19) 

If  we  use  this  transformation: 


c7-cc+  s7 •  cs+  cl  —0(  10 ) 


sin(q)= 


2-x 


(3.6.20) 


1+  x2 


cos (q)= 


1  — x 

1+  x2 


x=tan(-^) 
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We  get  a  square  equation: 

(3.6.21)  cc  ■  (1  -  x72)  +  cs  ■  2  ■  x7  +  cl  ■  (1  +  x72)  =  0  (11) 
The  2  solutions  are: 

x=  cs ±V (~cl2+  cc2+  cs2) 

(3.6.22)  cl  -cc 

The  q7  solution  will  be: 


(3.6.23)  q7 =atan2(2-x  ,1  —  x1)(l3) 

Using  q7  solution  and  (5y)  we  can  get: 

(3.6.24)  c5-py5+  s5- pz5—y67— y89-c7+  z8-s7=0(l4) 

We  can  use  manipulation  done  in  (10)  -  (13)  to  get  q5. 

Dividing  (5x)  by  (5z)  we  get  2  options: 

If  the  expression 

(3.6.25)  (z7+  c7-z8+  s7-y89)>  0 

Is  true,  then: 


(3.6.26) 

If  not  then: 

(3.6.27) 

Using  (2x)  we  get: 


q6=atan2(px ,  pz5-c5—  py5-s5){  15) 
q6  =  atan2 ( —  px , — ( pz5  c5  —  py5 ■  s5)) ( 16 ) 


(3.6.28)  c9'Sx— s9-ax— s6-s7=0(17) 
We  can  use  manipulation  done  in  (10)  -  (13)  to  get  q9. 


We  can  divide  (1  [2,1])  by  (1[2,3])  to  get  q8 
If  s7>0  then: 


(3.6.29) 
If  not: 


q8  —  atan2(ny-c5+  nzs5  ,-s9-(sy-c5+  sz-s5)—c9-(ay-c5+  oz*  sS ))( 18) 


q8=atan2(—(nyc5+  nzs5)  ,s9[sy-c5+  sz-s5)+  c9-(ayc5+  azs5))(  19) 


(3.6.30) 

We  had  here  3  square  equations  that  will  give  as  8  solutions;  from  these  8  solutions  we  will 
find  the  right  solution  by  calculation  of  error  from  the  wanted  position  and  orientation. 


2.6.5  Joint  coordinate  systems 


\^Pob 


il 
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To  create  a  common  language  which  all  the  modules  will  communicate  throw  we  define  the 
pelvis  as  the  major  link.  Because  our  numeric  method  inverse  and  forward  kinematics  works 
from  the  shoulder  to  the  hand  we  had  to  perform  a  manipulation  to  integrate  with  the  input 
(IK  from  pelvis  to  hand). 

We  define  now  the  solution  results  from  a  simple  matrix  manipulation: 


rp  shoulder  _ , 

(  rp  Pelvis  N 

|  1  rp  Pelvis 

^  Hand  ~  1 

\  Hand  , 

f  ±  Shoulder 

FIGURE  3.6.2:  T  shoulder  -  hand 


-j 

_ r*  °  Distribution  A: 


41 


Approved  for  public  release;  distribution  is  unlimited. 


DARPA  —  Tactical  Technology  Office  (TTO) 

AWARD  NO.  FA8655-12-1-2143 

DARPA-BAA- 12-39  -  “DARPA  Robotics  Challenge” 

Project  Name:  ROBIL 

FIGURE  3.6.3:  Object  grasping  and  motion 

2.6.6  Task:  Integrating  Dexterity  with  Vision 
Description: 

The  main  problem  of  this  integration  is  to  find  the  position  and  orientation  of  an  object 
relative  to  the  pelvis.  In  order  to  find  it  an  object's  position  and  orientation  which  the 
robot  is  capable  to  locate  and  grasp  (with  single  hand)  was  pre-defined.  After  this  zero 
position  was  set  the  robots  vision  could  find  a  transformation  (rotation  and  translation) 
from  the  zero  position  to  its  current  position.  This  transformation  matrix  was  manipulated 
on  the  hand's  initial  inverse  kinematics  such  that  the  new  solution  brings  the  hand  to  grasp 
exactly  on  the  same  place  on  the  object  zero  position. 

Method: 

1)  Pre-Definition  of  the  zero  position  of  the  Hose  (Dexterity  side): 

Method  of  trial  and  error  was  implemented  such  a  way  we  created  a  Task  3  world  and 
launch  files  that  brings  up  the  robot  stand  next  to  the  hose.  After  the  simulation  start  up  a 
sequence  of  IK  points  brought  the  hand  to  grasp  the  hose.  The  last  point  defines  the  "zero 
position".  At  this  stage  the  vision  should  detect  the  hose  so  that  it  returns  the  identity 
matrix  when  asked  for  the  position  of  the  hose. 
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FIGURE  3.6.4:  Zero  position  definition 


FIGURE  3.6.5:  Lifting  from  zero  position 


2)  Definition  of  un-known  position  of  the  hose: 

In  an  unknown  situation  the  vision  knows  to  send  transformation  from  the  initial  position 
to  the  hose  current  position  in  pelvis  coordinate.  The  dexterity  modules  should  multiply 
the  returned  matrix  with  the  left  side  of  the  matrix  generated  from  arm  zero  position 
(constant  matrix).  Using  straight  forward  equation  we  can  get  the  position  and  orientation 
and  send  them  to  the  action  modules. 

p  Pelvis  p  position  0  V  p  Pelvis 

1  Hand  1  Current  position  I  1  Hand 


^  position  0 
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3  Conclusions 


Looking  back  at  the  last  9  months,  ROBIL  group  has  transformed  from  11  Pi's  and  35 
students  coming  from  5  different  institutes,  into  1  united  group,  focused  on  autonomous 
robotics  development.  Apart  from  the  administrative  difficulty,  we  faced  hard  challenges 
regarding  movement  control,  perception,  decision  making,  and  integration  in  a  challenging 
environment,  a  parallel  simulator  development  and  in  a  very  short  time.  Although  we  didn't 
have  the  chance  to  demonstrate  all  our  developed  capabilities  during  the  VRC,  we  have  made 
a  huge  step  forward  and  was  not  too  far  from  performing  equally  to  the  winning  groups.  We 
believe  that  given  another  3-4  more  weeks  of  efficient  work  we  could  demonstrate  all  of  our 
planned  capabilities.  Concluding  this,  ROBIL  will  surely  facilitate  the  spirit  of  DARPA  and 
this  challenge  to  pursuit  autonomous  abilities  of  humanoid  robots  control  and  behavior,  and 
to  become  a  global  leader  in  advanced  robotic  software  and  hardware  development. 
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5  List  of  Symbols,  Abbreviations,  and  Acronyms 

BDI:  Belief-desired-intension  software  model 
COM  or  CoM:  Center  off  Mass 
CPG:  Central-Pattern  Generator 
FLC:  Fuzzy  Logic  Controller 
HMI:  Human  Machine  Interface 
HTN:  Hierarchical  Task  Network 
LiDAR:  Light  Detection  and  Ranging 
OCD:  Operational  Concept  Definition 
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OCU:  Operator  Control  Unit 
TLD:  Track,  Learn  and  Detect  algorithm 
VV&T:  Verification,  Validation  and  Testing 
ZMP:  Zero-Moment  point 
Symbols  list 

xd,yd  ~  Position  of  desired  destination. 

xcar,  Vcar  ~  Position  of  the  car 

dear  ~  Car's  azimuth 

6Car  ~  Car's  orientation  rate 

de  —  Error  in  orientation 

de  —  Distance  to  way  point 

v  —  Velocity 

(3  —  Steering  wheel  angle 
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